clc
clear all


syms t1 t2 t3 t4 l1 l2 l3 l4 lt
%{
T_b1 = [ cos(t1) -sin(t1) 0 0;
         sin(t1) cos(t1) 0 0;
         0 0 1 l1;
         0 0 0 1]
  %}      
%t_12 = [ cos(t2) -sin(



% pg 359 IK iteratve test

l1 = 1
l2 = 1
T_req = [1; 1]      % Required XY coords


% Build symbolic matrices
T = [ l1*cos(t1) + l2*cos(t1 + t2); l1*sin(t1)+l2*sin(t1+t2)]
J = jacobian(T, [t1 t2])
J_inv = inv(J)

q = zeros(2,4)
q(:,1) = [pi/3; -pi/3]      % Initial Guess

for i=2:10
    t1 = q(1, i-1)
    t2 = q(2, i-1)
    
    T_tmp = eval(T)                 % Eval the residue
    delta_T = T_req - T_tmp
    J_tmp = eval(J)                 % find the jacobian of it
    J_inv_tmp = eval(J_inv)         % find the inverse jacobian
    
    q(:,i) = q(:,i-1) + J_inv_tmp*delta_T       %Calculate the next iteration

end

  
%{
% Params
P = [ 0 0 l1 t1
      90 l2 0 t2
      0 l3 0 t3
      0 l4 0 t4
      0 lt 0 0]
  
%{
 P = [ 
      0 l3 0 t3
      0 l4 0 t4
      0 lt 0 0] 
%}  
  
for i=1:5
    al= degtorad(P(i,1))
    a = P(i,2)
    d = P(i,3)
    t = P(i,4)
    
    T(:,:,i)=[cos(t)               -sin(t)                   0              a;
        sin(t)*cos(al)        cos(t)*cos(al)          -sin(al)        -sin(al)*d;
        sin(t)*sin(al)        cos(t)*sin(al)          cos(al)         cos(al)*d;
        0                     0                       0               1];
    
end


Tr=eye(4);
for i=1:5
    Tr=Tr*T(:,:,i);
end

simplify(Tr)
%}
    